using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;

namespace XnaDevRu.Bullet
{
	public static class MatrixOperations
	{
		public static void SetRotation(ref Matrix m, Quaternion q)
		{
			float d = q.LengthSquared();
			BulletDebug.Assert(d != 0);
			float s = 2f / d;
			float xs = q.X * s, ys = q.Y * s, zs = q.Z * s;
			float wx = q.W * xs, wy = q.W * ys, wz = q.W * zs;
			float xx = q.X * xs, xy = q.X * ys, xz = q.X * zs;
			float yy = q.Y * ys, yz = q.Y * zs, zz = q.Z * zs;
			m = new Matrix(1 - (yy + zz), xy - wz, xz + wy, 0,
							xy + wz, 1 - (xx + zz), yz - wx, 0,
							xz - wy, yz + wx, 1 - (xx + yy), 0,
							m.M41, m.M42, m.M43, 1);
		}

		public static Quaternion GetRotation(Matrix m)
		{
			Quaternion q = new Quaternion();

			float trace = m.M11 + m.M22 + m.M33;

			if (trace > 0)
			{
				float s = (float)Math.Sqrt(trace + 1);
				q.W = s * 0.5f;
				s = 0.5f / s;

				q.X = (m.M32 - m.M23) * s;
				q.Y = (m.M13 - m.M31) * s;
				q.Z = (m.M21 - m.M12) * s;
			}
			else
			{
				int i = m.M11 < m.M22 ?
					(m.M22 < m.M33 ? 2 : 1) :
					(m.M11 < m.M33 ? 2 : 0);
				int j = (i + 1) % 3;
				int k = (i + 2) % 3;

				float s = (float)Math.Sqrt(MathHelper.GetElement(m, i, i) - MathHelper.GetElement(m, j, j) - MathHelper.GetElement(m, k, k) + 1);
				MathHelper.SetElement(ref q, i, s * 0.5f);
				s = 0.5f / s;

				q.W = (MathHelper.GetElement(m, k, j) - MathHelper.GetElement(m, j, k)) * s;
				MathHelper.SetElement(ref q, j, (MathHelper.GetElement(m, j, i) + MathHelper.GetElement(m, i, j)) * s);
				MathHelper.SetElement(ref q, k, (MathHelper.GetElement(m, k, i) + MathHelper.GetElement(m, i, k)) * s);
			}

			return q;
		}

		public static Matrix Scaled(Matrix m, Vector3 v)
		{
			return new Matrix(  m.M11 * v.X, m.M12 * v.Y, m.M13 * v.Z, 0,
								m.M21 * v.X, m.M22 * v.Y, m.M23 * v.Z, 0,
								m.M31 * v.X, m.M32 * v.Y, m.M33 * v.Z, 0,
								0, 0, 0, 1);
		}
		
		public static Matrix Multiply(Matrix a, Matrix b)
		{
			/*return btMatrix3x3(
			m2.tdot(0, m1[0]), m2.tdot(1, m1[0]), m2.tdot(2, m1[0]),
			m2.tdot(0, m1[1]), m2.tdot(1, m1[1]), m2.tdot(2, m1[1]),
			m2.tdot(0, m1[2]), m2.tdot(1, m1[2]), m2.tdot(2, m1[2]));*/
			return new Matrix(
				Dot(b, 0, MathHelper.GetRow(a, 1)), Dot(b, 1, MathHelper.GetRow(a, 1)), Dot(b, 2, MathHelper.GetRow(a, 1)), 0,
				Dot(b, 0, MathHelper.GetRow(a, 2)), Dot(b, 1, MathHelper.GetRow(a, 2)), Dot(b, 2, MathHelper.GetRow(a, 2)), 0,
				Dot(b, 0, MathHelper.GetRow(a, 3)), Dot(b, 1, MathHelper.GetRow(a, 3)), Dot(b, 2, MathHelper.GetRow(a, 3)), 0,
				0, 0, 0, 1);
		}

		public static float Dot(Matrix m, int c, Vector3 v) 
		{
			return MathHelper.GetElement(m, 0, c) * v.X + MathHelper.GetElement(m, 1, c) * v.Y + MathHelper.GetElement(m, 2, c) * v.Z;
		}

		public static Matrix Transpose(Matrix m)
		{
			return new Matrix(	m.M11, m.M21, m.M31, 0,
								m.M12, m.M22, m.M32, 0,
								m.M13, m.M23, m.M33, 0,
								0, 0, 0, 1);
		}
	}
}
